﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Xna.Framework;

namespace StaticView.Algorithm
{
    // AccelerationSeparator: algorithm used when there're no gyroscope
    class AccelerationSeparator
    {
        Vector3 Magnetic; // The magnetic field vector (reliable)
        Vector3 Gravity; // The gravity vector

        // The low pass filters for acceleration input
        LowPassFilter[] GravityFilter = new LowPassFilter[3]; // Filter for gravity seperation
        LowPassFilter[] AccelerationFilter = new LowPassFilter[3]; // Filter for acceleration smoothing

        public AccelerationSeparator(Vector3 Magnetic, Vector3 Gravity, float dt)
        {
            // Initial value of B and G
            this.Magnetic = Magnetic;
            this.Gravity = Gravity;

            // Initialize the filters
            Matrix G = new Matrix(Gravity);
            for (int i = 0; i < 3; i++)
            {
                GravityFilter[i] = new LowPassFilter(G[i, 0], 0.25f, dt);
                AccelerationFilter[i] = new LowPassFilter(0, 0.05f, dt);
            }
        }

        // Return the acceleration value, and also the rotation quaternion
        public Vector3 getAcceleration(Vector3 magnetic, Vector3 accelerometer, out Quaternion Q)
        {
            Matrix I = new Matrix(accelerometer); // Input of the filter
            Matrix O = new Matrix(3, 1); // Output of the filter

            // Filter the accelerometer output
            for (int i = 0; i < 3; i++)
                O[i, 0] = GravityFilter[i].Next(I[i, 0]);

            // Low Pass Filtered output
            Vector3 LowPassAcce = O.ToVector3();

            /* Separating process -----------------------------------------------------------
             *      B: Magnetic field (magnetic)
             *      A: Accelerometer output (LowPassAcce)
             *      G: Gravity (gravity)
             */

            // Angle (Cos) between 2 initial vectors
            float Angle = Vector3.Dot(Magnetic, Gravity) / (Magnetic.Length() * Gravity.Length());

            // Magnetic field Direction
            Vector3 DirB = magnetic;
            DirB.Normalize();

            // Normal of plane containing B and A
            Vector3 Normal = Vector3.Cross(magnetic, LowPassAcce);

            // Quaternion to Rotate B to G
            Quaternion Qbg = Quaternion.CreateFromAxisAngle(Normal, (float)Math.Acos(Angle));

            // Rotate to get G
            Vector3 gravity = Vector3.Transform(DirB, Qbg);
            gravity.Normalize();
            gravity *= Gravity.Length();

            Vector3 acceleration = accelerometer - gravity;

            // End separating process -------------------------------------------------------

            // Smooth the output
            I = new Matrix(acceleration);
            O = new Matrix(3, 1);

            for (int i = 0; i < 3; i++)
                O[i, 0] = AccelerationFilter[i].Next(I[i, 0]);

            Q = QuaternionHelper.QuaternionFromTwoVectors(Magnetic, Gravity, magnetic, gravity);

            return O.ToVector3();
        }

    }
}
